/*
 * @Description: In User Settings Edit
 * @Author: your name
 * @Date: 2019-10-12 17:50:55
 * @LastEditTime: 2019-10-13 00:14:42
 * @LastEditors: Please set LastEditors
 */
#include <opencv2/opencv.hpp>
#include <thread>
#include <unistd.h>
#include "buff_detect.h"

using namespace std;
using namespace cv;

int main() { 

    BuffDetector buff_detector;

    // 能量机关调试参数
    namedWindow("BuffParam");
    createTrackbar("buff_gray_th", "BuffParam", &buff_detector.gray_th_, 255);
    createTrackbar("buff_color_th", "BuffParam", &buff_detector.color_th_, 255);
    createTrackbar("buff_offset_x_","BuffParam",&buff_detector.buff_offset_x_,200);
    createTrackbar("buff_offset_y_","BuffParam",&buff_detector.buff_offset_y_,200);
    createTrackbar("world_offset_x","BuffParam",&buff_detector.world_offset_x_,1000);
    createTrackbar("fire_max_cnt","BuffParam",&buff_detector.auto_control.fire_task.max_cnt_,200);
    createTrackbar("reset_cnt","BuffParam",&buff_detector.auto_control.reset_task.max_cnt_,200);
    createTrackbar("repeat_time","BuffParam",&buff_detector.auto_control.fire_task.repeat_time,2000);
    createTrackbar("fire","BuffParam",&buff_detector.auto_control.fire_task.fire_flag,1);
    createTrackbar("repeat fire","BuffParam",&buff_detector.auto_control.fire_task.repeat_fire_flag,1); 
    
    VideoCapture cap("../Videos/Sivir2.avi");
    Mat image; 
    float angle_x = 0.0, angle_y = 0.0, distance =  0.0;
    int command = 0;
    OtherParam other_param;

    buff_detector.readXML();

    while (1) { 
        bool status = cap.read(image);
        if (!status) 
            break; 

        imshow("rawimg", image); 
        command = buff_detector.BuffDetectTask(image, other_param); 
        if(command)
        {
            buff_detector.getAngle(angle_x, angle_y);
            distance = buff_detector.getDistance();
            printf("Distance: %f, Yaw: %f, Pitch: %f\n", distance, angle_x, angle_y); 
        }
        waitKey(1); 
    }
}